#include "system.h"
#include "user_bsp.h"


//Run_Step_St Run_Step_E=Step_Begin;
//Fly_Mode_St Fly_Mode_E=Mode_One;
Mode_One_St_Act_t Mode_One_St_Act = Mode_One_St_Act_Climb;
Mode_Two_St_Act_t Mode_Two_St_Act = Mode_Two_St_Act_Climb;
Fly_Mode_St Fly_Mode_E=Mode_Two;
volatile uint16_t En_Wait_time=0;

void Action_Update()
{
	  switch(Fly_Mode_E)
    {
        case Mode_One:       Ctrl_Update_Act_Mode_One();    break;
        case Mode_Two:   	 Ctrl_Update_Act_Mode_Two();    break;
        case Mode_Three:   	 Ctrl_Update_Act_Mode_Three();  break;
    }
}

#define Rol_Pos_Err 10
#define Pit_Pos_Err 10
#define Yaw_Pos_Err 10
#define Expect_Height 80
void Ctrl_Update_Act_Mode_One(void)
{

    switch(Mode_One_St_Act)
    {
        case Mode_One_St_Act_Climb:
        {
            if( ABS(Fly_Status_t.Height-Expect_Height)<10)
            {		
								Wait_time_EN_OFF(200);
								if(En_Wait_time==0)
								{
									Mode_One_St_Act = Mode_One_St_Act_Stable;
									Wait_time_EN_OFF(0);									 
								}
								
            }
        }
        break;
        case Mode_One_St_Act_Stable:
        {
            if(( ABS(SysPID[Rol].err) < Rol_Pos_Err)
               &&(ABS(SysPID[Pit].err) < Pit_Pos_Err))
						{
				//				Buzzer_ON;
								Wait_time_EN_OFF(800);
								if(En_Wait_time==0)
								{
									Mode_One_St_Act = Mode_One_St_Act_Right_Move;
									recOkFlash = 1;
									Wait_time_EN_OFF(0);
								}

						}
        }
        break;
        case Mode_One_St_Act_Right_Move:
        {
            if(Feature_Point_E==Top_Left_RightAngle)
            {
								Wait_time_EN_OFF(40);
								if(En_Wait_time==0)
								{
										if(Feature_Point_E==Top_Left_RightAngle)
										{
											Mode_One_St_Act = Mode_One_St_Act_FirRight_Angle_Stable;
										}	
										Wait_time_EN_OFF(0);
								}
								
								
						}
        }
        break;
        case Mode_One_St_Act_FirRight_Angle_Stable://
        {
            if(  (ABS(SysPID[Rol].err) < Rol_Pos_Err)
                &&  ABS(SysPID[Pit].err) < Pit_Pos_Err)
						{
		//						Buzzer_ON;
								Wait_time_EN_OFF(800);
								if(En_Wait_time==0)
								{
									 Mode_One_St_Act = Mode_One_St_Act_Front_Move;//½øÈëÏÂÒ»¸ö×´Ì¬
									 recOkFlash = 1;
		//							Buzzer_OFF;
									Wait_time_EN_OFF(0);
								}
								
						}
        }
        break;
        case Mode_One_St_Act_Front_Move:
        {
            if(Feature_Point_E==Bottom_Left_RightAngle)
            {
							Wait_time_EN_OFF(40);
							if(En_Wait_time==0)
							{
                Mode_One_St_Act = Mode_One_St_Act_SecRight_Angle_Stable;

								Wait_time_EN_OFF(0);
							}

        }
        break;
				
				 case Mode_One_St_Act_SecRight_Angle_Stable:
        {
					if((ABS(SysPID[Rol].err) < Rol_Pos_Err
                      &&  ABS(SysPID[Pit].err) < Pit_Pos_Err))
					{		
						
	//						Buzzer_ON;
							Wait_time_EN_OFF(800);
							if(En_Wait_time==0)
							{
                Mode_One_St_Act = Mode_One_St_Act_Left_First_Move;//½øÈëÏÂÒ»¸ö×´Ì¬
                recOkFlash = 1;
	//							Buzzer_OFF;
								Wait_time_EN_OFF(0);
							}
							
					}
        }
        break;
				
				 case Mode_One_St_Act_Left_First_Move:
        {
            if(Feature_Point_E==Bottom_Left_Right_Circle)
            {
								Wait_time_EN_OFF(40);
								if(En_Wait_time==0)
								{
									Mode_One_St_Act = Mode_One_St_Act_Front_Circle_Stable;

									Wait_time_EN_OFF(0);
                }
						}
        }
        break;


				case Mode_One_St_Act_Front_Circle_Stable:
        {
					if(
                  (SysPID[Rol].err < Rol_Pos_Err)
                &&  (SysPID[Pit].err < Pit_Pos_Err)
               )
					{
		//				Buzzer_ON;
						Wait_time_EN_OFF(800);
						if(En_Wait_time==0)
						{
							Mode_One_St_Act = Mode_One_St_Act_Left_Second_Move;
							recOkFlash = 1;
		//					Buzzer_OFF;
							Wait_time_EN_OFF(0);
						}
					}
        }
        break;

				case Mode_One_St_Act_Left_Second_Move:
        {

            if(Feature_Point_E==Bottom_Right_RightAngle)
            {
								Wait_time_EN_OFF(40);
								if(En_Wait_time==0)
								{
									Mode_One_St_Act = Mode_One_St_Act_ThirdRight_Angle_Stable;

									Wait_time_EN_OFF(0);
                }
						}
        }
        break;
				

				case Mode_One_St_Act_ThirdRight_Angle_Stable:
        {
					if(
                (ABS(SysPID[Rol].err) < Rol_Pos_Err)
                &&  (ABS(SysPID[Pit].err)< Pit_Pos_Err))
					{
	//					Buzzer_ON;
						Wait_time_EN_OFF(800);
						if(En_Wait_time==0)
						{
							Mode_One_St_Act = Mode_One_St_Act_Back_Move;//½øÈëÏÂÒ»¸ö×´Ì¬
	//						Buzzer_OFF;
							Wait_time_EN_OFF(0);
						}
					}
        }
        break;
				
				case Mode_One_St_Act_Back_Move:
        {
            if(Feature_Point_E==Top_Right_RightAngle)
            {
							Wait_time_EN_OFF(40);
							if(En_Wait_time==0)
							{
									Mode_One_St_Act = Mode_One_St_Act_FouthRight_Angle_Stable;
									Wait_time_EN_OFF(0);
              }
						}
        }
        break;
				
				case Mode_One_St_Act_FouthRight_Angle_Stable:
        {
					if(
                  (ABS(SysPID[Rol].err) < Rol_Pos_Err
                &&  ABS(SysPID[Pit].err) < Pit_Pos_Err))
					{
		//				Buzzer_ON;
						Wait_time_EN_OFF(800);
						if(En_Wait_time==0)
						{
							Mode_One_St_Act = Mode_One_St_Act_Right_Move_To_Circle;//½øÈëÏÂÒ»¸ö×´Ì¬
		//					Buzzer_OFF;
							recOkFlash = 1;
							Wait_time_EN_OFF(0);
						}
					}
        }
        break;
				
				case Mode_One_St_Act_Right_Move_To_Circle:
        {
            if(Feature_Point_E==Top_Left_Right_Circle)
            {
							Wait_time_EN_OFF(40);
							if(En_Wait_time==0)
							{
									Mode_One_St_Act = Mode_One_St_Act_End_Circle_Stable;
									Wait_time_EN_OFF(0);
              }
						}
        }
        break;
				
				case Mode_One_St_Act_End_Circle_Stable:
				{
						if(
				ABS(SysPID[Rol].err) < Rol_Pos_Err
                &&  ABS(SysPID[Pit].err) < Rol_Pos_Err
               )
					{
						Wait_time_EN_OFF(800);
						if(En_Wait_time==0)
						{
							Mode_One_St_Act = Mode_One_St_Act_Land;
							recOkFlash = 1;
							Wait_time_EN_OFF(0);
						}
					}
				}
				break;
				
				case Mode_One_St_Act_Land:
				{
					
				}
				break;
    }
	}
}


void Ctrl_Update_Act_Mode_Two(void)
{
   switch(Mode_Two_St_Act)
   {
       case Mode_Two_St_Act_Climb:
       {
           if( ABS(Fly_Status_t.Height-Expect_Height)<10)
           {
				Wait_time_EN_OFF(200);
				if(En_Wait_time==0)
				{
					Mode_Two_St_Act = Mode_Two_St_Act_Start_Stable;
					Wait_time_EN_OFF(0);
				}

           }
       }
       break;
       case Mode_Two_St_Act_Start_Stable:
       {
             if(( ABS(SysPID[Rol].err) < Rol_Pos_Err)
                &&(ABS(SysPID[Pit].err) < Pit_Pos_Err))
			{
				Wait_time_EN_OFF(800);
				if(En_Wait_time==0)
				{
					Mode_Two_St_Act = Mode_Two_St_Act_Right_Move;
					recOkFlash = 1;
					Wait_time_EN_OFF(0);
				}

			}
         }
       break;
       case Mode_Two_St_Act_Right_Move:
       {
           if(Feature_Point_E==Top_Left_RightAngle)
           {
								Wait_time_EN_OFF(40);
								if(En_Wait_time==0)
								{
										if(Feature_Point_E==Top_Left_RightAngle)
										{
											Mode_Two_St_Act = Mode_Two_St_Act_Right_Stable;
										}
										Wait_time_EN_OFF(0);
								}


						}
       }
       break;
       case  Mode_Two_St_Act_Right_Stable:
	      {
	            if(  (ABS(SysPID[Rol].err) < Rol_Pos_Err)
	                &&  ABS(SysPID[Pit].err) < Pit_Pos_Err)
				{
									Wait_time_EN_OFF(800);
									if(En_Wait_time==0)
									{
										Mode_Two_St_Act = Mode_Two_St_Act_TrackingLine;
										recOkFlash = 1;
										Wait_time_EN_OFF(0);
									}

				}
	            else
	            {
	            	Wait_time_EN_OFF(0);
	            	Wait_time_EN_OFF(800);
	            }
	        }
       break;
       case Mode_Two_St_Act_TrackingLine:
       {
           if(Feature_Point_E==Bottom_Left_RightAngle)
           {
								Wait_time_EN_OFF(40);
								if(En_Wait_time==0)
								{
										if(Feature_Point_E==Bottom_Left_RightAngle)
										{
											Mode_Two_St_Act = Mode_Two_St_Act_Left_Stable;
										}
										Wait_time_EN_OFF(0);
								}


						}
       }
       break;
       case Mode_Two_St_Act_Left_Stable:
       {
			if((ABS(SysPID[Rol].err) < Rol_Pos_Err
                &&  ABS(SysPID[Pit].err) < Pit_Pos_Err))
		    {
				Wait_time_EN_OFF(800);
				if(En_Wait_time==0)
				{
					Mode_Two_St_Act = Mode_Two_St_Act_Left_Move;
                   recOkFlash = 1;
                   Wait_time_EN_OFF(0);
				}

		    }
          }
       break;
       case Mode_Two_St_Act_Left_Move:
       {
           if(Feature_Point_E==Bottom_Left_Right_Circle)
           {
				Wait_time_EN_OFF(40);
				if(En_Wait_time==0)
				{
					Mode_Two_St_Act = Mode_Two_St_Act_End_Stable;
                    Wait_time_EN_OFF(0);
               }
			}
       }
       break;
       case Mode_Two_St_Act_End_Stable:
		{
			if(
				ABS(SysPID[Rol].err) < Rol_Pos_Err
				&&  ABS(SysPID[Pit].err) < Rol_Pos_Err
			)
			{
				Wait_time_EN_OFF(800);
				if(En_Wait_time==0)
				{
					Mode_Two_St_Act = Mode_TWO_St_Act_Land;
					recOkFlash = 1;
					Wait_time_EN_OFF(0);
				}
			}
		}
		break;
       case Mode_TWO_St_Act_Land:
       break;
       default:break;

   }
}

void Ctrl_Update_Act_Mode_Three(void)
{

}
 void SysCtrl_Update_PID_Change(void)
{
    switch(Fly_Mode_E)
    {
        case Mode_One:       Ctrl_Update_Act_Mode_One_Pid_Change();        break;
        case Mode_Two:   	 Ctrl_Update_Act_Mode_Two_Pid_Change();    break;
//        case Mode_Three:   	 Ctrl_Update_Act_Mode_Three_Pid_Change();    break;
    }
}

void Ctrl_Update_Act_Mode_One_Pid_Change()
{

    switch(Mode_One_St_Act)
    {
        case Mode_One_St_Act_Right_Move:
        {
            SysPID[Rol].Out_Int16 = 8;
        }
        break;
				 case Mode_One_St_Act_Front_Move:
        {
            SysPID[Pit].Out_Int16 = -8;
        }
        break;
				 case Mode_One_St_Act_Back_Move:
        {
            SysPID[Pit].Out_Int16 = 12;
        }
        break;

				 case Mode_One_St_Act_Left_First_Move:
				 {
					SysPID[Rol].Out_Int16 = -4;
				 }
				 break;
				 case Mode_One_St_Act_Left_Second_Move:
        {
            SysPID[Rol].Out_Int16 = -8;
        }
        break;
			 case Mode_One_St_Act_Right_Move_To_Circle:
        {
            SysPID[Rol].Out_Int16 = 8;
        }
        break;
    }
}

void Ctrl_Update_Act_Mode_Two_Pid_Change(void)
{
    switch(Mode_Two_St_Act)
    {
        case Mode_Two_St_Act_TrackingLine:
        {
            SysPID[Pit].Out_Int16 = 6;
        }
        break;
        case Mode_Two_St_Act_Right_Move:
        {
            SysPID[Rol].Out_Int16 = 6;
        }
        break;
        case Mode_Two_St_Act_Left_Move:
        {
            SysPID[Rol].Out_Int16 = -6;
        }
        break;
    }

}


void Wait_time_EN_OFF(uint16_t time)
{
	static u8 sign=0;
	if(sign==0&&time==0)
	{
		return;
	}
	else if(sign==0)
	{
		En_Wait_time=time;
		sign=1;
	}
	else if(sign==1&&time==0)
		sign=0;

}


//YesNo_t Is_On_Node(YesNo_t Front_valid, YesNo_t Back_valid, YesNo_t Left_valid,YesNo_t Right_valid)
//{
//	
//}

